/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2024 INRIA.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/*!\file vertex_extraction.c
 * \brief interface to various LP solvers
 */

#include "vertex_extraction.h"

#include <stdio.h>   // for printf
#include <stdlib.h>  // for exit, EXIT_FAILURE

#include "SiconosConfig.h"  // for HAS_ONE_LP_SOLVER // IWYU pragma: keep
#ifdef __cplusplus
#undef restrict
#define restrict __restrict
#endif

//#define DEBUG_STDOUT
//#define DEBUG_MESSAGES
//#include "siconos_debug.h"

#ifdef HAS_ONE_LP_SOLVER
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-function"
#include "NumericsMatrix.h"
#include "lp_lib.h"

void siconos_find_vertex(const polyhedron* P, unsigned size, lapack_int* basis) {
  unsigned nrows = P->size_ineq;
  assert(P->H->matrix0);
  double* restrict H = P->H->matrix0;
  double* restrict K = P->K;
  lprec* lp;
  lp = make_lp(nrows, nrows + size);
  set_verbose(lp, CRITICAL);
  set_minim(lp);

  int* rowno = (int*)malloc(nrows * sizeof(int));
  for (unsigned i = 0; i < nrows; ++i) {
    rowno[i] = i + 1;
    set_mat(lp, i + 1, size + i + 1, -1.0);
    // assert(K[nrows-1-i] >= 0.0);
    set_mat(lp, 0, size + 1 + i, fabs(K[nrows - 1 - i]) + 0.0001 * (rand() % 100));
    set_constr_type(lp, i + 1, EQ);
  }
  //    set_constr_type(lp, nrows+1, GE);

  for (unsigned i = 1, j = 0; i <= size; ++i, j += nrows) {
    set_columnex(lp, i, nrows, &H[j], rowno);
    set_unbounded(lp, i);
  }
  set_rh_vec(lp, &K[-1]);
  //  set_mat(lp, nrows+1, size+1, 1.0);

#ifdef DEBUG_STDOUT
  print_lp(lp);
#endif
  /* Solve the LP */
  int info = solve(lp);
  if (info != 0) {
    printf("find_vertex_lpsolve: failure in the LP solver: info = %d\n", info);
    exit(EXIT_FAILURE);
  }

  int* lp_basis = NULL;
  if (sizeof(lapack_int) != sizeof(int)) {
    lp_basis = (int*)malloc((nrows + 1) * sizeof(int));
  } else {
    lp_basis = (int*)basis;
  }

  get_basis(lp, lp_basis, FALSE);

  if (sizeof(lapack_int) != sizeof(int)) {
    for (size_t i = 0; i < (nrows + 1); ++i) {
      basis[i] = (lapack_int)lp_basis[i];
    }
  }

#ifdef DEBUG_STDOUT
  print_solution(lp, 3);
#endif
  delete_lp(lp);
  free(rowno);
}

#pragma GCC diagnostic pop

#else

void siconos_find_vertex(const polyhedron* P, unsigned size, int* basis) {
  printf("You need to compile the lp_solve support");
  exit(EXIT_FAILURE);
}

#endif
